//MPU6050
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

Adafruit_MPU6050 mpu;

/*计算偏移量*/
float i;                                    //计算偏移量时的循环次数
float ax_offset = 0, ay_offset = 0;         //x,y轴的加速度偏移量
float gx_offset = 0, gy_offset = 0;         //x,y轴的角速度偏移量

/*参数*/
float rad2deg = 57.29578;                   //弧度到角度的换算系数
float roll_v = 0, pitch_v = 0;              //换算到x,y轴上的角速度

/*定义微分时间*/
float now = 0, lasttime = 0, dt = 0;        //定义微分时间

/*三个状态，先验状态，观测状态，最优估计状态*/
float gyro_roll = 0, gyro_pitch = 0;        //陀螺仪积分计算出的角度，先验状态
float acc_roll = 0, acc_pitch = 0;          //加速度计观测出的角度，观测状态
float k_roll = 0, k_pitch = 0;              //卡尔曼滤波后估计出最优角度，最优估计状态

/*误差协方差矩阵P*/
float e_P[2][2] ={{1,0},{0,1}};             //误差协方差矩阵，这里的e_P既是先验估计的P，也是最后更新的P

/*卡尔曼增益K*/
float k_k[2][2] ={{0,0},{0,0}};             //这里的卡尔曼增益矩阵K是一个2X2的方阵


//可用输出引脚32
int gpio_useful[18] ={32,25,26,27,14,12,13,23,22,21,19,18,5,17,16,4,2,15};

//Mux ocntrol pins
int s0 = 14;//33引脚坏了改14
int s1 = 25;
int s2 = 26;
int s3 = 27;

//Mux in "SIG"
int SIG_pin_OUT = 33;

//Third order matrix
int c1 = 36;
int c2 = 39;
int c3 = 34;
int c4 = 35;

//int r12 = 13;
//int r11 = 2;
//int r10 = 12;
//int r9 = 3;
//int r8 = 11;
//int r7 = 4;
//int r6 = 10;
//int r5 = 5;
//int r4 = 9;
//int r3 = 6;
//int r2 = 8;
//int r1 = 7;

int SIG_pin_IN[] = {c1,c2,c3,c4};

//连接wifi后登陆MQTT，然后每1s上报一次数据(数据每次加1)
#include <WiFi.h>
#include <PubSubClient.h>
#include <ArduinoJson.h>

#define WIFI_SSID "HONOR V20"//wifi名称
#define WIFI_PASSWORD "9876543211"//wifi密码

//注册设备的ID和密钥
#define device_id "61e2e1eec7fb24029b0ca162_pressure"
#define secret "cx123456"

//MQTT三元组
#define ClientId "61e2e1eec7fb24029b0ca162_pressure_0_0_2022020915"
#define Username "61e2e1eec7fb24029b0ca162_pressure"
#define Password "3c7ebd9780ac56de6f50c30eb95426b9544479b5ce7a0e9ac84915254d77f93d"

#define MQTT_Address "a1619e3b36.iot-mqtts.cn-north-4.myhuaweicloud.com"
#define MQTT_Port 1883
#define Iot_link_Body_Format "{\"services\":[{\"service_id\":\"fuwu\",\"properties\":{%s"
//4Pin
/*
{
  "services": [
    {
      "service_id": "fuwu",
      "properties": {
        "pressure": 39, //1
        "frs1":11,      //2
        "frs2":11,      //3
        "frs3":11       //4
      }
    }
  ]
}
*/
//{"services":[{"service_id":"fuwu","properties":{"pressure": 39}}]}
#define Iot_link_MQTT_Topic_Report "$oc/devices/" device_id "/sys/properties/report"

WiFiClient myesp8266Client;

//----------------------------------------------------------------------p48data
int P48_data_array[12][4] = {
    {0,0,0,0},
    {0,0,0,0},
    {0,0,0,0},
    {0,0,0,0},
    {0,0,0,0},
    {0,0,0,0},
    {0,0,0,0},
    {0,0,0,0},
    {0,0,0,0},
    {0,0,0,0},
    {0,0,0,0},
    {0,0,0,0}
  };

//mpu6050 

PubSubClient client(myesp8266Client);
//int data_temp=1;   测试数据，已废弃
void setup() {
  pinMode(s0, OUTPUT);
  pinMode(s1, OUTPUT);
  pinMode(s2, OUTPUT);
  pinMode(s3, OUTPUT);
  pinMode(SIG_pin_OUT, OUTPUT);

  pinMode(c1,INPUT);
  pinMode(c2,INPUT);
  pinMode(c3,INPUT);
  pinMode(c4,INPUT);

  digitalWrite(s0,LOW);
  digitalWrite(s1,LOW);
  digitalWrite(s2,LOW);
  digitalWrite(s3,LOW);
  digitalWrite(SIG_pin_OUT,LOW);  
  
  // put your setup code here, to run once:
  Serial.begin(115200);
  WIFI_Init();
  MQTT_Init();

  /*打开串口和实现I2C通信*/
  Wire.begin();//SDA->20,SCL->21,可以根据情况自行修改

  /*判断是否连接到MPU6050并且初始化*/
  while (!mpu.begin())
  {
    Serial.println("Failed to find MPU6050 chip");
  }
  Serial.println("MPU6050 Found!");
  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);//加速度量程±2G
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);//角速度量程±250°/s
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);//采样频率21Hz

  //计算偏移量
  for (i = 1; i <= 2000; i++) {
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);//获取加速度、角速度、温度
    ax_offset = ax_offset + a.acceleration.x;//计算x轴加速度的偏移总量
    ay_offset = ay_offset + a.acceleration.y;//计算y轴加速度的偏移总量
    gx_offset = gx_offset + g.gyro.x;
    gy_offset = gy_offset + g.gyro.y;
  }
  ax_offset = ax_offset / 2000; //计算x轴加速度的偏移量
  ay_offset = ay_offset / 2000; //计算y轴加速度的偏移量
  gx_offset = gx_offset / 2000; //计算x轴角速度的偏移量
  gy_offset = gy_offset / 2000; //计算y轴角速度的偏移量
//  delay(100);
}
void loop() {
  /*计算微分时间*/
  now = millis();                           //当前时间(ms)
  dt = (now - lasttime) / 1000.0;           //微分时间(s)
  lasttime = now;                           //上一次采样时间(ms)

  Serial.println(lasttime);
  
  /*获取角速度和加速度 */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);//获取加速度、角速度、温度

  /*step1:计算先验状态*/
  /*计算x,y轴上的角速度*/
  roll_v = (g.gyro.x-gx_offset) + ((sin(k_pitch)*sin(k_roll))/cos(k_pitch))*(g.gyro.y-gy_offset) + ((sin(k_pitch)*cos(k_roll))/cos(k_pitch))*g.gyro.z;//roll轴的角速度
  pitch_v = cos(k_roll)*(g.gyro.y-gy_offset) - sin(k_roll)*g.gyro.z;//pitch轴的角速度
  gyro_roll = k_roll + dt*roll_v;//先验roll角度
  gyro_pitch = k_pitch + dt*pitch_v;//先验pitch角度

  /*step2:计算先验误差协方差矩阵P*/
  e_P[0][0] = e_P[0][0] + 0.0025;//这里的Q矩阵是一个对角阵且对角元均为0.0025
  e_P[0][1] = e_P[0][1] + 0;
  e_P[1][0] = e_P[1][0] + 0;
  e_P[1][1] = e_P[1][1] + 0.0025;

  /*step3:更新卡尔曼增益K*/
  k_k[0][0] = e_P[0][0]/(e_P[0][0]+0.3);
  k_k[0][1] = 0;
  k_k[1][0] = 0;
  k_k[1][1] = e_P[1][1]/(e_P[1][1]+0.3);

  /*step4:计算最优估计状态*/
  /*观测状态*/
  //roll角度
  acc_roll = atan((a.acceleration.y - ay_offset) / (a.acceleration.z))*rad2deg;
  //pitch角度
  acc_pitch = -1*atan((a.acceleration.x - ax_offset) / sqrt(sq(a.acceleration.y - ay_offset) + sq(a.acceleration.z)))*rad2deg;
  /*最优估计状态*/
  k_roll = gyro_roll + k_k[0][0]*(acc_roll - gyro_roll);
  k_pitch = gyro_pitch + k_k[1][1]*(acc_pitch - gyro_pitch);

  /*step5:更新协方差矩阵P*/
  e_P[0][0] = (1 - k_k[0][0])*e_P[0][0];
  e_P[0][1] = 0;
  e_P[1][0] = 0;
  e_P[1][1] = (1 - k_k[1][1])*e_P[1][1];

  //打印角度
  Serial.print("roll: ");
  Serial.print(k_roll);
  Serial.print(",");
  Serial.print("pitch: ");
  Serial.println(k_pitch);
  

  //姿态可视化
//  Serial.print(k_roll);
//  Serial.print("/");
//  Serial.println(k_pitch);
  
  for(int i=0; i < 12 ; i++){
      readMux(i);
  }
  Serial.println(" ");
  Serial.println(" ");
  Serial.println(" ");

  // put your main code here, to run repeatedly:
  MQTT_POST();
//  delay(1000);
//  data_temp++;  测试模拟数据，已废弃
}

//连接wifi
void WIFI_Init()
{
    WiFi.mode(WIFI_STA);
    WiFi.begin(WIFI_SSID,WIFI_PASSWORD);
    while(WiFi.status()!=WL_CONNECTED)
    {
      delay(1000);
      Serial.println("WiFi Not Connect");
    }
    Serial.println("WiFi Connected OK!");
}

//连接MQTT
void MQTT_Init()
{
  client.setServer(MQTT_Address,MQTT_Port);
  while(!client.connected())
  {
    client.connect(ClientId,Username,Password);
    delay(1000);
    Serial.println(client.state());
    Serial.println("MQTT Connected Fail!");
  }
  Serial.println("MQTT Connected OK!");
}

void MQTT_POST()
{
  char properties[1024];
  char jsonBuf[1024];

/*  
  sprintf(properties,"\"pressure\":%d,\"frs1\":%d,\"frs2\":%d,\"frs3\":%d,\"P48\":%d}}]}",
     P48_data_array[0][0],P48_data_array[0][1],P48_data_array[0][2],P48_data_array[0][3],
     P48_data_array[1][0],P48_data_array[1][1],P48_data_array[1][2],P48_data_array[1][3],
     P48_data_array[2][0],P48_data_array[2][1],P48_data_array[2][2],P48_data_array[2][3],
     P48_data_array[3][0],P48_data_array[3][1],P48_data_array[3][2],P48_data_array[3][3],
     P48_data_array[4][0],P48_data_array[4][1],P48_data_array[4][2],P48_data_array[4][3],
     P48_data_array[5][0],P48_data_array[5][1],P48_data_array[5][2],P48_data_array[5][3],
     P48_data_array[6][0],P48_data_array[6][1],P48_data_array[6][2],P48_data_array[6][3],
     P48_data_array[7][0],P48_data_array[7][1],P48_data_array[7][2],P48_data_array[7][3],
     P48_data_array[8][0],P48_data_array[8][1],P48_data_array[8][2],P48_data_array[8][3],
     P48_data_array[9][0],P48_data_array[9][1],P48_data_array[9][2],P48_data_array[9][3],
     P48_data_array[10][0],P48_data_array[10][1],P48_data_array[10][2],P48_data_array[10][3],
     P48_data_array[11][0],P48_data_array[11][1],P48_data_array[11][2],P48_data_array[11][3]
     ); //将frsReading格式化进properties中
*/
  sprintf(properties,
      "\"R1C1\":%d,\"R1C2\":%d,\"R1C3\":%d,\"R1C4\":%d,"
      "\"R2C1\":%d,\"R2C2\":%d,\"R2C3\":%d,\"R2C4\":%d,"
      "\"R3C1\":%d,\"R3C2\":%d,\"R3C3\":%d,\"R3C4\":%d,"
      "\"R4C1\":%d,\"R4C2\":%d,\"R4C3\":%d,\"R4C4\":%d,"
      "\"R5C1\":%d,\"R5C2\":%d,\"R5C3\":%d,\"R5C4\":%d,"
      "\"R6C1\":%d,\"R6C2\":%d,\"R6C3\":%d,\"R6C4\":%d,"
      "\"R7C1\":%d,\"R7C2\":%d,\"R7C3\":%d,\"R7C4\":%d,"
      "\"R8C1\":%d,\"R8C2\":%d,\"R8C3\":%d,\"R8C4\":%d,"
      "\"R9C1\":%d,\"R9C2\":%d,\"R9C3\":%d,\"R9C4\":%d,"
      "\"R10C1\":%d,\"R10C2\":%d,\"R10C3\":%d,\"R10C4\":%d,"
      "\"R11C1\":%d,\"R11C2\":%d,\"R11C3\":%d,\"R11C4\":%d,"
      "\"R12C1\":%d,\"R12C2\":%d,\"R12C3\":%d,\"R12C4\":%d,"
      "\"Roll\":%f,\"Pitch\":%f,\"Yaw\":%f,\"time\":%f"
    "}}]}",
    P48_data_array[0][0],P48_data_array[0][1],P48_data_array[0][2],P48_data_array[0][3],
            P48_data_array[1][0],P48_data_array[1][1],P48_data_array[1][2],P48_data_array[1][3],
    P48_data_array[2][0],P48_data_array[2][1],P48_data_array[2][2],P48_data_array[2][3],
            P48_data_array[3][0],P48_data_array[3][1],P48_data_array[3][2],P48_data_array[3][3],
    P48_data_array[4][0],P48_data_array[4][1],P48_data_array[4][2],P48_data_array[4][3],
            P48_data_array[5][0],P48_data_array[5][1],P48_data_array[5][2],P48_data_array[5][3],
    P48_data_array[6][0],P48_data_array[6][1],P48_data_array[6][2],P48_data_array[6][3],
            P48_data_array[7][0],P48_data_array[7][1],P48_data_array[7][2],P48_data_array[7][3],
    P48_data_array[8][0],P48_data_array[8][1],P48_data_array[8][2],P48_data_array[8][3],
            P48_data_array[9][0],P48_data_array[9][1],P48_data_array[9][2],P48_data_array[9][3],
    P48_data_array[10][0],P48_data_array[10][1],P48_data_array[10][2],P48_data_array[10][3],
            P48_data_array[11][0],P48_data_array[11][1],P48_data_array[11][2],P48_data_array[11][3],
    k_roll,k_pitch,0,lasttime
  );
  
  Serial.println(properties);
  sprintf(jsonBuf,Iot_link_Body_Format,properties);     //将properties格式化进Iot_link_Body_Format中存储在jsonBuf中
  client.publish(Iot_link_MQTT_Topic_Report, jsonBuf);  //publish
  Serial.println(Iot_link_MQTT_Topic_Report);
  Serial.println(jsonBuf);
  Serial.println("MQTT Publish OK!");
}

void readMux(int channel){
//  int controlPin[] = {r1,r2,r3,r4,r5,r6,r7,r8,r9,r10,r11,r12};

//  int muxChannel[12][12] = {
//    {1,0,0,0,0,0,0,0,0,0,0,0},
//    {0,1,0,0,0,0,0,0,0,0,0,0},
//    {0,0,1,0,0,0,0,0,0,0,0,0},
//    {0,0,0,1,0,0,0,0,0,0,0,0},
//    {0,0,0,0,1,0,0,0,0,0,0,0},
//    {0,0,0,0,0,1,0,0,0,0,0,0},
//    {0,0,0,0,0,0,1,0,0,0,0,0},
//    {0,0,0,0,0,0,0,1,0,0,0,0},
//    {0,0,0,0,0,0,0,0,1,0,0,0},
//    {0,0,0,0,0,0,0,0,0,1,0,0},
//    {0,0,0,0,0,0,0,0,0,0,1,0},
//    {0,0,0,0,0,0,0,0,0,0,0,1},
//  };
//
//  //loop through the 4 sig
//  for(int i=0; i < 12 ; i++){
//    digitalWrite(controlPin[i], muxChannel[channel][i]);
//  }

  //---------------
  writeMux(channel);
//  delay(20);
    
  for(int i=0;i<4;i++){
    int val = analogRead(SIG_pin_IN[i]);
    //Serial.print("Value at ");
    //Serial.print(pad[channel][i]+" ");
    //Serial.print(" is : ");
    P48_data_array[channel][i] = val;
    Serial.print(val);
    Serial.print("/");
    
  }
  Serial.println();
//  return val;
}

void writeMux(int channel){
  int controlPin[] = {s0,s1,s2,s3};

//  扩展板序
//  int muxChannel[12][4] = {
//    {0,0,0,0}, //channel 0  ---11
//    {1,0,0,0}, //channel 1  ---9
//    {0,1,0,0}, //channel 2  ---7
//    {1,1,0,0}, //channel 3  ---5
//    {0,0,1,0}, //channel 4  ---3
//    {1,0,1,0}, //channel 5  ---1
//    {0,1,1,0}, //channel 6  ---2
//    {1,1,1,0}, //channel 7  ---4
//    {0,0,0,1}, //channel 8  ---6
//    {1,0,0,1}, //channel 9  ---8
//    {0,1,0,1}, //channel 10 ---10
//    {1,1,0,1}  //channel 11 ---12
//  };

  // 传感器序
  int muxChannel[12][4] = {
    {1,0,1,0}, //channel 5  ---1
    {0,1,1,0}, //channel 6  ---2
    {0,0,1,0}, //channel 4  ---3
    {1,1,1,0}, //channel 7  ---4
    {1,1,0,0}, //channel 3  ---5
    {0,0,0,1}, //channel 8  ---6
    {0,1,0,0}, //channel 2  ---7
    {1,0,0,1}, //channel 9  ---8
    {1,0,0,0}, //channel 1  ---9
    {0,1,0,1}, //channel 10 ---10
    {0,0,0,0}, //channel 0  ---11
    {1,1,0,1}  //channel 11 ---12
  };
  

  for(int i = 0; i < 4 ; i++){
    digitalWrite(controlPin[i],muxChannel[channel][i]);
  }
  
  digitalWrite(SIG_pin_OUT,HIGH);
}
// Mux多路复用
